import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class master {
	TachoPilot p;
	int angle = 45;
	wallfollow WALLE;
	public static void main(String[] args)
	{
		// Nieuwe muurvolger starten.
		new master().go();
	}

	private void go(){
		p = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C,true);
		p.setMoveSpeed(12f);
		WALLE = new wallfollow(p);
//		changeWallSide(false);
		while(true){
			System.out.println("Driving");
			try{
				Thread.sleep(1000);
			}catch(InterruptedException e){
			}
			WALLE.pause();

			System.out.println("Stopped");
			try{
				Thread.sleep(1000);
			}catch(InterruptedException e){
			}
			
			WALLE.restart();

			System.out.println("Restarted");
			try{
				Thread.sleep(1000);
			}catch(InterruptedException e){
			}
		}
	}

	private void changeWallSide(boolean left){
		WALLE.pause();
		WALLE.setSide(false);
		Motor.B.rotate(WALLE.getSide()*angle);
		try{
			Thread.sleep(1000);
		}catch(InterruptedException e){
		}
		WALLE.restart();
	}
}
